/*=========================================================================

  Program:   IGI Pivot Calibration
  Module:  $RCSfile: PivotCalibration.cxx,v $
  Language:  C++
  Date:    $Date: 2012-09-18 19:52:31 $
  Version:   $Revision: 1.15 $

  Copyright (c) ISC  Insight Software Consortium.  All rights reserved.
  See IGSTKCopyright.txt or http://www.igstk.org/copyright.htm for details.

   This software is distributed WITHOUT ANY WARRANTY; without even
   the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
   PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include <QtGui>
#include "PivotCalibration.moc"
#include "PivotCalibration.h"
#include <sstream>

#include "igstkTransformFileWriter.h"
#include "igstkRigidTransformXMLFileWriter.h"

#include "itksys/SystemTools.hxx"

#include "../IGIConfigurationData.h"

PivotCalibration::PivotCalibration()
: m_initialized(false)
{ 
  m_GUI.setupUi(this);
  this->CreateActions();

  m_GUIQuit  = false;

  m_delay = 5000;
  //create the class that actually does all the work
  this->m_pivotCalibration = igstk::PivotCalibration::New();

  //add observer for initialization events
  this->m_InitializationObserver = InitializationObserverType::New();
  this->m_InitializationObserver->SetCallbackFunction(
    this,
    &PivotCalibration::OnInitializationEvent );

  this->m_pivotCalibration->AddObserver(
                   igstk::PivotCalibration::InitializationFailureEvent(),
                         this->m_InitializationObserver );
  this->m_pivotCalibration->AddObserver(
                   igstk::PivotCalibration::InitializationSuccessEvent(),
                         this->m_InitializationObserver );

  //add observer for the events generated during pivot calibration
  this->m_CalibrationObserver = CalibrationObserverType::New();
  this->m_CalibrationObserver->SetCallbackFunction(
    this,
    &PivotCalibration::OnCalibrationEvent );

  this->m_pivotCalibration->AddObserver(
                     igstk::PivotCalibration::DataAcquisitionEvent(),
                          this->m_CalibrationObserver );
  this->m_pivotCalibration->AddObserver(
                    igstk::PivotCalibration::DataAcquisitionEndEvent(),
                          this->m_CalibrationObserver );
  this->m_pivotCalibration->AddObserver(
                    igstk::PivotCalibration::CalibrationSuccessEvent(),
                          this->m_CalibrationObserver );
  this->m_pivotCalibration->AddObserver(
                    igstk::PivotCalibration::CalibrationFailureEvent(),
                          this->m_CalibrationObserver );

  this->m_TransformToObserver = TransformToObserver::New();
  this->m_PivotPointObserver = PivotPointObserver::New();
  this->m_RMSEObserver = RMSEObserver::New();

  //settings for the stream that accumulates the calibration information
  this->m_calibrationInformationStream.precision(3);
  this->m_calibrationInformationStream.setf(ios::fixed);

  //create transform observer
  this->m_TransformToObserver = TransformToObserver::New();
  //create RMSE observer
  this->m_RMSEObserver = RMSEObserver::New();

  //create calibration success and failure observer
  this->m_CalibrationSuccessFailureObserver = CalibrationSuccessFailureObserverType::New();  
  this->m_CalibrationSuccessFailureObserver->SetCallbackFunction( 
  this,
  &PivotCalibration::OnCalibrationSuccessFailureEvent );

  this->m_pivotCalibration->AddObserver(
  igstk::PivotCalibration::CalibrationSuccessEvent(),
  this->m_CalibrationSuccessFailureObserver );

  this->m_pivotCalibration->AddObserver(
  igstk::PivotCalibration::CalibrationFailureEvent(),
  this->m_CalibrationSuccessFailureObserver );
  
  //create error observer
  this->m_errorObserver = TrackingErrorObserver::New();

  //create tracker
  this->m_Tracker = igstk::ArucoTracker::New();

  //observe errors generated by the tracker
  this->m_Tracker->AddObserver( igstk::TrackerOpenErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerInitializeErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerStartTrackingErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerStopTrackingErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerUpdateStatusErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerCloseErrorEvent(),
                this->m_errorObserver );
  this->m_Tracker->AddObserver( igstk::TrackerUpdateStatusErrorEvent(),
                this->m_errorObserver );

  // get current application path
  QString path = QApplication::applicationDirPath();
  path.truncate(path.lastIndexOf("/Programs"));
  m_CurrentPath = path + "/Programs";
  
  m_ConfigDir = path + "/" + IGIConfigurationData::CONFIGURATION_FOLDER;
  
  nrOfTransformations = 300;
  m_GUI.CalibrateButton->setEnabled(false);
  m_GUI.CheckPositionButton->setEnabled(false);

  QTimer *pulseTimer = new QTimer();
  connect(pulseTimer, SIGNAL(timeout()), this, SLOT(PulseTimerEvent()));
  pulseTimer->start(10);
}

void PivotCalibration::PulseTimerEvent()
{
  igstk::PulseGenerator::CheckTimeouts();
}

void PivotCalibration::CreateActions()
{
  connect(m_GUI.QuitPushButton, SIGNAL(clicked()), this, SLOT(OnQuitAction()));

  connect(m_GUI.InitializeButton, SIGNAL(clicked()), this,
      SLOT(OnInitializationAction()));

  connect(m_GUI.CalibrateButton, SIGNAL(clicked()), this,
      SLOT(OnCalibrationAction()));

  connect(m_GUI.TransformationsSlider, SIGNAL(valueChanged(int)), this, SLOT(setTransformationsValue(int)));

  connect(m_GUI.TransformationsSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setTransformationsValue(int)));
  connect(m_GUI.helpButton, SIGNAL(clicked()), this, SLOT(showHelp()));

  connect(m_GUI.CheckPositionButton, SIGNAL(clicked()), this, SLOT(CheckMarkerPosition()));
}

void PivotCalibration::setTransformationsValue(int  value)
{
  nrOfTransformations = value;
  m_GUI.TransformationsSlider->setValue(value);
  m_GUI.TransformationsSpinBox->setValue(value);
}

void
PivotCalibration::OnInitializationAction()
{
  m_GUI.LEDlabel->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));   

  unsigned int delay =
      static_cast<unsigned int>( 5 );
  unsigned int numberOfFrames =
      static_cast<unsigned int>( nrOfTransformations );

  //if already initialized, shutdown everything and reinitialize
  if( this->m_initialized )
  {
    this->m_initialized = false;
    this->m_Tracker->RequestStopTracking();
    if( this->m_errorObserver->Error() )
    {
      this->m_errorObserver->ClearError();
      return;
    }
    this->m_Tracker->RequestClose();
    if( this->m_errorObserver->Error() )
    {
      this->m_errorObserver->ClearError();
      return;
    }
  }

  // create tracked tool and configure it
  this->m_tool = igstk::ArucoTrackerTool::New();
  
  this->m_tool->RequestSetMarkerName(m_GUI.idBox->currentText().toInt());
  this->m_tool->RequestConfigure();

  this->m_Tracker->RequestSetFrequency( 20 );

  // Set marker size in mm
  this->m_Tracker->SetMarkerSize(50);
	
  std::string calibFile = m_ConfigDir.toStdString()
	                      + "/"
	                      + IGIConfigurationData::CAMERA_CALIBRATION_FILENAME;

  if(!this->m_Tracker->SetCameraParametersFromYAMLFile(calibFile))
  {
    QMessageBox::warning(0,"Warning",
    "Error loading camera calibration parameters from yml file.",
    QMessageBox::Ok );
    return;
  }

  //open tracker communication
  this->m_Tracker->RequestOpen();
  if( this->m_errorObserver->Error() )
  {
    this->m_errorObserver->ClearError();
    return;
  }

  //attach the tracker tool
  this->m_tool->RequestAttachToTracker( this->m_Tracker );
  if( this->m_errorObserver->Error() )
  {
    this->m_errorObserver->ClearError();
    return;
  }
  
  //start tracking
  this->m_Tracker->RequestStartTracking();
  if( this->m_errorObserver->Error() )
  {
    this->m_errorObserver->ClearError();
    return;
  }

  this->m_initialized = true;

  this->RequestSetDelay( delay );

  igstk::TrackerTool * genericTrackerTool = this->m_tool.GetPointer();
  this->RequestInitialize(numberOfFrames, genericTrackerTool );
}

void
PivotCalibration::OnCalibrationSuccessFailureEvent( itk::Object *caller,
                const itk::EventObject & event )
{
  if( dynamic_cast< const igstk::PivotCalibration::CalibrationSuccessEvent * >
   (&event) )
  {
  }
  else if( dynamic_cast< const igstk::PivotCalibration::CalibrationFailureEvent * >
       (&event) )
  {
  }
}

void
PivotCalibration::TrackingErrorObserver::Execute(
  itk::Object *caller,
  const itk::EventObject & event )
{
  std::map<std::string,std::string>::iterator it;
  std::string className = event.GetEventName();
  it = this->m_ErrorEvent2ErrorMessage.find( className );
  if( it != this->m_ErrorEvent2ErrorMessage.end() )
  {
  this->m_ErrorOccured = true;
  QMessageBox::warning(0,"Warning",
    (*it).second.c_str(),
    QMessageBox::Ok );
  }
}

void
PivotCalibration::TrackingErrorObserver::Execute(
  const itk::Object *caller,
  const itk::EventObject & event )
{
  const itk::Object * constCaller = caller;
  this->Execute( constCaller, event );
}

void
PivotCalibration::TrackingErrorObserver::ClearError()
{
  this->m_ErrorOccured = false;
}

bool
PivotCalibration::TrackingErrorObserver::Error()
{
  return this->m_ErrorOccured;
}

PivotCalibration
::TrackingErrorObserver::TrackingErrorObserver()
: m_ErrorOccured( false )
{
  //tracker errors
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerOpenErrorEvent().GetEventName(),
    "Error opening tracker communication." ) );

  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerInitializeErrorEvent().GetEventName(),
   "Error initializing tracker." ) );

  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerStartTrackingErrorEvent().GetEventName(),
   "Error starting tracking." ) );

  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerStopTrackingErrorEvent().GetEventName(),
   "Error stopping tracking." ) );

  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerCloseErrorEvent().GetEventName(),
   "Error closing tracker communication." ) );

  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>(
    igstk::TrackerUpdateStatusErrorEvent().GetEventName(),
   "Error updating transformations from tracker." ) );
}

void PivotCalibration::CheckMarkerPosition()
{
  cv::Mat currentImage = m_Tracker->GetCurrentVideoFrame();
  cv::imshow("video",currentImage);
  cv::waitKey(2000);
  cv::destroyWindow("video");
}

void PivotCalibration::OnQuitAction()
{
  this->close();
  m_GUIQuit = true;
}

bool PivotCalibration::HasQuit( )
{
  return m_GUIQuit;
}

void
PivotCalibration::RequestInitialize( unsigned int n,
                     igstk::TrackerTool::Pointer trackerTool )
{
  m_GUI.CalibrateButton->setEnabled(true);
  m_GUI.CheckPositionButton->setEnabled(true);
  //show the description of the current tool on the UI

  m_GUI.LEDlabel->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));

  m_GUI.Result->setText("Data acquisition starts 5 seconds after pressing 'Calibrate'.");

  //try to initialize
  this->m_pivotCalibration->RequestInitialize( n, trackerTool );
}

void
PivotCalibration::OnCalibrationAction()
{
  m_GUI.InitializeButton->setEnabled(false);
  m_GUI.CheckPositionButton->setEnabled(false);

  std::ostringstream msg;
  for(unsigned int i=m_delay; i>0; i-=1000 )
  {
    m_delay-=1000;
    msg << "Data acquisition starts in "<<(int)(i/1000)<<" seconds.";
    m_GUI.Result->setText(QString(msg.str().c_str()));
    QApplication::processEvents();
    igstk::PulseGenerator::Sleep(1000);
    msg.str("");
  }
  msg.str("Collecting poses...");
  m_GUI.Result->setText(QString(msg.str().c_str()));
  QApplication::processEvents();

  m_GUI.CalibrateButton->setEnabled(false);
  m_delay = 5000;
  this->m_pivotCalibration->RequestComputeCalibration();
}

void
PivotCalibration::RequestSetDelay( unsigned int delayInSeconds )
{
  this->m_delay = delayInSeconds*1000;
}

void
PivotCalibration::RequestCalibrationTransform()
{
  this->m_pivotCalibration->RequestCalibrationTransform();
}

void
PivotCalibration::RequestCalibrationRMSE()
{
  this->m_pivotCalibration->RequestCalibrationRMSE();
}

void
PivotCalibration::RequestPivotPoint()
{
  this->m_pivotCalibration->RequestPivotPoint();
}

void
PivotCalibration::OnInitializationEvent(itk::Object *caller,
                                        const itk::EventObject & event )
{
  if( dynamic_cast< const
        igstk::PivotCalibration::InitializationSuccessEvent * > (&event) )
  {
    //activate "Calibrate" button
    m_GUI.CalibrateButton->setEnabled(true);
  }
  else if( dynamic_cast<
    const igstk::PivotCalibration::InitializationFailureEvent * > (&event) )
  {
    QMessageBox::warning(0,"Warning",
      "Failed to initialize pivot calibration.\n Check that tool is valid.",
      QMessageBox::Ok );
  }
}

void
PivotCalibration::OnCalibrationEvent( itk::Object *caller,
                        const itk::EventObject & event )
{
  std::ostringstream msg;

  m_GUI.Result->clear();

  if( const igstk::PivotCalibration::DataAcquisitionEvent *evt =
     dynamic_cast< const igstk::PivotCalibration::DataAcquisitionEvent * > (&event) )
  {
    m_GUI.CalibrationProgressBar->setValue( evt->Get()*100 );
    QApplication::processEvents();
  }
  else if( dynamic_cast<
          const igstk::PivotCalibration::DataAcquisitionEndEvent * > (&event) )
  {
    m_GUI.CalibrationProgressBar->setValue( 0 );
  }
  else if( const igstk::PivotCalibration::CalibrationFailureEvent *evt =
       dynamic_cast<
          const igstk::PivotCalibration::CalibrationFailureEvent * > (&event) )
  {
    m_GUI.CalibrateButton->setEnabled(true);
    m_GUI.CalibrationProgressBar->setValue( 0 );
    m_GUI.InitializeButton->setEnabled(true);
    m_GUI.CheckPositionButton->setEnabled(true);

    msg.str("");
    msg<<"Calibration failed:\n\t"<<evt->Get();
    QMessageBox::warning(0,"Warning",
    msg.str().c_str(),
    QMessageBox::Ok );
  }
  //calibration succeeded, get all the information
  //(Transformation, Pivot Point, RMSE) and display it
  else if( dynamic_cast<
          const igstk::PivotCalibration::CalibrationSuccessEvent * > (&event) )
  {
    m_GUI.CalibrationProgressBar->setValue( 0 );

    this->m_calibrationInformationStream.str("");
    igstk::PivotCalibration* calib = dynamic_cast< igstk::PivotCalibration *> (caller);

    //get the transformation
    unsigned long observerID = calib->AddObserver(
                      igstk::CoordinateSystemTransformToEvent(),
                            this->m_TransformToObserver );
    calib->RequestCalibrationTransform();
    calib->RemoveObserver( observerID );
    igstk::Transform transform =
             this->m_TransformToObserver->GetTransformTo().GetTransform();
    igstk::Transform::VersorType v = transform.GetRotation();
    igstk::Transform::VectorType t = transform.GetTranslation();

    this->m_calibrationInformationStream<<"Transform:\n";
    this->m_calibrationInformationStream<<"quaternion: ";
    this->m_calibrationInformationStream<<v.GetX()<<"  "<<v.GetY()<<"  "<<v.GetZ()<<"  "<<v.GetW()<<"\n";
    this->m_calibrationInformationStream<<"translation: ";
    this->m_calibrationInformationStream<<t[0]<<"\t"<<t[1]<<"\t"<<t[2]<<"\n";
          //get the pivot point
    observerID = calib->AddObserver( igstk::PointEvent(),
      this->m_PivotPointObserver );
    calib->RequestPivotPoint();
    calib->RemoveObserver( observerID );
    igstk::EventHelperType::PointType pnt =
                    this->m_PivotPointObserver->GetPivotPoint();
    this->m_calibrationInformationStream<<"Pivot point: ";
    this->m_calibrationInformationStream<<pnt[0]<<"\t"<<pnt[1]<<"\t"
                                   <<pnt[2]<<"\n";
    //get the RMS error
    observerID = calib->AddObserver( igstk::DoubleTypeEvent(),
                               this->m_RMSEObserver );
    calib->RequestCalibrationRMSE();
    calib->RemoveObserver( observerID );
    double rmse = this->m_RMSEObserver->GetRMSE();
    this->m_calibrationInformationStream<<"RMSE: "<< rmse;
                      
    m_GUI.Result->append(this->m_calibrationInformationStream.str().c_str());
    m_GUI.CalibrateButton->setEnabled(true);
    m_GUI.InitializeButton->setEnabled(true);
    m_GUI.CheckPositionButton->setEnabled(true);

	  std::string file = m_ConfigDir.toStdString()
                         + "/"
                         + IGIConfigurationData::TOOL_CALIBRATION_FILENAME;

    igstk::PrecomputedTransformData::Pointer transformationData = 
    igstk::PrecomputedTransformData::New();

    //get transformation description
    std::ostringstream descriptionStream;
    descriptionStream<<"Pivot calibration for tool ";
    descriptionStream<<this->m_tool->GetTrackerToolIdentifier();
    
    //get current date
    std::string estimationDate = 
    itksys::SystemTools::GetCurrentDateTime( "%Y %b %d %H:%M:%S" );
    
    transformationData->RequestInitialize( &transform, estimationDate,
                       descriptionStream.str(), rmse );

    //setup the writer
    igstk::TransformFileWriter::Pointer transformFileWriter = 
    igstk::TransformFileWriter::New();

    igstk::TransformXMLFileWriterBase::Pointer xmlFileWriter;
    xmlFileWriter = igstk::RigidTransformXMLFileWriter::New();

    WriteFailureObserverType::Pointer writeFailureObserver = 
    WriteFailureObserverType::New();
    writeFailureObserver->SetCallbackFunction( 
    this, 
    &PivotCalibration::OnWriteFailureEvent );

    transformFileWriter->AddObserver( igstk::TransformFileWriter::WriteFailureEvent() , 
                    writeFailureObserver );

    transformFileWriter->RequestSetWriter( xmlFileWriter );
    transformFileWriter->RequestSetData( transformationData,
                                         file );
    transformFileWriter->RequestWrite();

	  QMessageBox::information(this,windowTitle(),
        "Pointer tool successfully calibrated.\nCalibration file saved in: "
        + QString(file.c_str())
        + " [" + IGIConfigurationData::TOOL_CALIBRATION_FILENAME + "]");
  }
}

void 
PivotCalibration::OnWriteFailureEvent( itk::Object * itkNotUsed(caller),
                            const itk::EventObject & itkNotUsed(event) )
{
  QMessageBox::critical(this,windowTitle(),
        "Failed writing calibration data to file.");
}

void PivotCalibration::showHelp()
{
  QDialog* helpBox = new QDialog(this);
  helpBox->setWindowTitle("Help");

  QTextBrowser* browser = new QTextBrowser(helpBox); 
  browser->setSource(QUrl::fromLocalFile(m_CurrentPath + "/READMEPivotCalibration.html"));
  browser->setWindowTitle("Help");

  QPushButton* okButton = new QPushButton(helpBox);
  connect(okButton, SIGNAL(clicked()), helpBox, SLOT(close()));
  okButton->setGeometry(QRect(150, 260, 100, 25));
  okButton->setText("Quit");

  QVBoxLayout* helpLayout = new QVBoxLayout(helpBox);
  helpLayout->addWidget(browser);
  helpLayout->addWidget(okButton);

  helpBox->resize(500,500);
  helpBox->show();
}
